Zero-Control Linearization-Based Steering for Sample-Based Planning of Dynamic Systems

We investigate steering and distance computations based on linearizations about zero-control trajectories for RRT-like motion planning of highly dynamic systems. With planning constrained to dynamically feasible trajectories, the concept of “straight” is no longer Euclidean but instead determined as the path with least cost to transfer from one state to another. This additional complexity transforms steering and nearest neighbor computations into optimal control problems for which numerical techniques are slow compared to solving Euclidean distance. However, the optimal control solutions can be approximated as Linear Quadratic Regulation or Tracking problem (LQR or LQT) through linearizations of the cost and dynamics as done in Glassman and Tedrake [2010]; Goretkin et al. [2013]; Perez et al. [2012]; Webb and van den Berg [2013], who all linearize about a single point—e.g. an explored state or a sampled state. In comparison, we propose linearizing about the zero-control trajectories. The advantage here is that the approximation remains valid for longer time-horizons at the expense of solving time-varying LQR instead of time-invariant LQR. This abstract and the subsequent interactive session workshop compares zero-control linearization and single point linearization distance based computations for RRT planning of a pendulum on a cart system. The system has four states which are the cart position, cart velocity, pendulum angle, and pendulum angular velocity. The example system is shown in Figure 1 along with trajectories explored by an RRT execution with a tree of 1000 vertices for differing max time horizons. As is qualitatively apparent from the figure, planning with longer time horizons results in a denser explored space for an equal number of vertices. This statement is quantified in Figure 2 which compares the number of vertices and execution time to discover a state within δ = 2 away from xgoal marked by the green star in Figure 1. Note that the start and goal states are upright, have zero velocity and start 6 meters apart. The results are discussed further later.